// Ted Carancho's MikroQuadAero - September 2008
// An Arudino based quadrocopter using the Sparkfun
// 5DOF IMU and the Pololu RC servo controller
// This version will look at only gyro measurements
// to provide flight stability

// ******************** Initialize Variables ********************
//Debug data
#define DEBUG 1
#define BAUD 38400

// Define pin assignments
#define LEDPIN 13
#define RESETPIN 12
#define THROTTLEPIN 2
#define ROLLPIN 3
#define PITCHPIN 4
#define YAWPIN 5
#define GEARPIN 6
#define AUXPIN 7
#define ROLLRATEPIN 4
#define PITCHRATEPIN 3
#define YAWRATEPIN 5

// Define servo assignments
#define FRONTCHANNEL 0
#define RIGHTCHANNEL 1
#define REARCHANNEL 2
#define LEFTCHANNEL 3

// Define transmitter commands
#define TIMEOUT 4500
volatile int throttle = 0;
volatile int roll = 0;
volatile int pitch = 0;
volatile int yaw = 0;
volatile int gear = 0;
volatile int aux= 0;
int rollCommand, pitchCommand, yawCommand;

// Define motor commands
unsigned int frontCommand = 0;
unsigned int rightCommand = 0;
unsigned int rearCommand = 0;
unsigned int leftCommand = 0;
unsigned int throttleMotorCommand = 0;
int rollMotorCommand = 0;
int pitchMotorCommand = 0;
int yawMotorCommand = 0;

// Gyro setup
// If AREF = 3.3V, then A/D is 931 at 3V and 465 = 1.5V
// Gyro range is from +/-500 deg/sec
float gyroMid = 465;
int rollGyroZero, pitchGyroZero, yawGyroZero;
int rollRate, pitchRate, yawRate;

// Scale transmitter commands (1000 to 2000) to gyro rate (0 to 931, or -465 to +465 when centered around 0)
// use y = mx + b
// m = (y2 - y1) / (x2 - x1)
// y2 = 465, y1 = -465, x2 = 2000, x1 = 1000
float mGyro = 0.93;
// b = y1 - m * x1
float bGyro = -1395;

// Scale transmitter commands (1000 to 2000) to motor commands (2000 to 4700)
// use y = mx + b
float mMotor = 2.7;
float bMotor = -700;

// Scale gyro rate (-465 to +465) to motor commands (2000 to 4700)
// use y = mx + b
float mMotorRate = 2.9;
float bMotorRate = 3348.5;

// Digital smoothing parameters
// Taken from http://www.arduino.cc/playground/Main/DigitalSmooth
#define FILTERSAMPLES 23               // filterSamples should be an odd number, no smaller than 3
int rollSmoothArray [FILTERSAMPLES];   // array for holding raw sensor values for roll 
int rollData, rollSmoothData;          // variables for roll data
int pitchSmoothArray [FILTERSAMPLES];  // array for holding raw sensor values for pitch 
int pitchData, pitchSmoothData;        // variables for pitch data
int yawSmoothArray [FILTERSAMPLES];    // array for holding raw sensor values for yaw 
int yawData, yawSmoothData;            // variables for yaw data

// PID Values
#define WINDUP_GUARD_GAIN 100.0
float pTerm, iTerm, dTerm; 
float pgain = 500;
float igain = 1;
float dgain = -450;
float iRollState = 0;
float lastRollPosition = 0;
float iPitchState = 0;
float lastPitchPosition = 0;
float iYawState = 0;
float lastYawPosition = 0;

// Communication
volatile char queryType = 'G';

// Interrupt Handler
volatile byte timeSlot;

// Timing
long previousTime = 0;
long currentTime = 0;
long deltaTime = 0;

// ******************** Setup MikroQuadAero ********************
void setup() {
  Serial.begin(BAUD);
  analogReference(EXTERNAL); // Current external ref is 3.3V
  pinMode(LEDPIN, OUTPUT);
  pinMode(RESETPIN, OUTPUT);  
  pinMode(THROTTLEPIN, INPUT);
  pinMode(ROLLPIN, INPUT);
  pinMode(PITCHPIN, INPUT);
  pinMode(YAWPIN, INPUT);
  digitalWrite(LEDPIN, LOW);
  
  // Calibrate gyros
  rollGyroZero = gyroMid - findZero(ROLLRATEPIN);
  pitchGyroZero = gyroMid - findZero(PITCHRATEPIN);
  yawGyroZero = gyroMid - findZero(YAWRATEPIN);
  
  // Reset Pololu servo controller
  digitalWrite(RESETPIN, LOW);
  delay(1000);
  digitalWrite(RESETPIN, HIGH);

  // Configure motors
  for (byte channel = 0; channel < 4; channel++)
    motorConfigure(channel);

  // Wait for ESC's to initialize
  delay(4000);

  // Setup interrupt to trigger on pin D3 (roll receiver pin)
  attachInterrupt(1, interruptHandler, RISING);

  // Enable MikroQuadAero ready LED
  digitalWrite(LEDPIN, HIGH);
  previousTime = millis();
}

// ******************** Main MikroQuadAero Loop ********************
void loop () {
  // Measure Timing
  currentTime = millis();
  deltaTime = currentTime - previousTime;
  previousTime = currentTime;

  // Read Sensors
  // Force values to be centered at 0
  rollRate = digitalSmooth(analogRead(ROLLRATEPIN), rollSmoothArray) + rollGyroZero - gyroMid;
  pitchRate = digitalSmooth(analogRead(PITCHRATEPIN), pitchSmoothArray) + pitchGyroZero - gyroMid;
  yawRate = digitalSmooth(analogRead(YAWRATEPIN), yawSmoothArray) + yawGyroZero - gyroMid;
  
  // Calculate Commanded Angles
  // Convert from PWM pulse length to gyro A/D value
  rollCommand = (mGyro * roll) + bGyro;
  pitchCommand = (mGyro * pitch) + bGyro;
  yawCommand = (mGyro * yaw) + bGyro;
  
  // Update PID
  throttleMotorCommand = (mMotor * throttle) + bMotor;
  rollMotorCommand = (mMotorRate * updatePID(rollRate, rollCommand, &lastRollPosition, &iRollState)) + bMotorRate;
  pitchMotorCommand = (mMotorRate * updatePID(pitchRate, pitchCommand, &lastPitchPosition, &iPitchState)) + bMotorRate;
  yawMotorCommand = (mMotorRate * updatePID(yawRate, yawCommand, &lastYawPosition, &iYawState)) + bMotorRate;   

  // Command Motors
  frontCommand = constrain((unsigned int)(throttleMotorCommand + pitchMotorCommand + yawMotorCommand), 2000, 4700);
  rearCommand = constrain((unsigned int)(throttleMotorCommand - pitchMotorCommand + yawMotorCommand), 2000, 4700);
  rightCommand = constrain((unsigned int)throttleMotorCommand + rollMotorCommand - yawMotorCommand, 2000, 4700);
  leftCommand = constrain((unsigned int)(throttleMotorCommand - rollMotorCommand - yawMotorCommand), 2000, 4700);
}

int findZero(int channel) {
  int zero = 0;
  for (int i=0; i< 64; i++) zero += analogRead(channel);
  return zero >> 6;
}  


